#include "plugins/action/initial_stage_action.hpp"

BT::NodeStatus InitialStage::onStart() {
    cnt_ = 0;
    tmp_sensor_data_filtered_={};
    start_millis_=0, current_millis_=0;
    buffer_.clear();
    std::cout << "InitialStage_onStart()" << std::endl;
    print_all_inports(true);
    this->onRunning();
    return BT::NodeStatus::RUNNING;
}
void InitialStage::filter_laser_ranging(std::vector<DataInfo> &data_buffer, int start)
{
    laser_ranging_max_ = 0;
    laser_ranging_min_ = 0;
    laser_ranging_sum_ = 0;
    for(int i = start; i < data_buffer.size(); i++)
    {
        if(laser_ranging_max_ < data_buffer.at(i).distance)
        {
            laser_ranging_max_ = data_buffer.at(i).distance;
        }

        if(laser_ranging_min_ > data_buffer.at(i).distance)
        {
            laser_ranging_min_ = data_buffer.at(i).distance;
        }

        laser_ranging_sum_ += data_buffer.at(i).distance;
    }

    double average = laser_ranging_sum_ / (data_buffer.size() - start);
    double temp = (laser_ranging_max_ - average) > (average - laser_ranging_min_) ? 
                (laser_ranging_max_ - average) : (average - laser_ranging_min_);
    maximum_threshold_ = (laser_ranging_max_ - average) / temp / 2;
    std::cout<<"start:"<<start<<std::endl;
    std::cout<<"maximum_threshold:"<<maximum_threshold_<<std::endl;
    std::cout<<"filter_buffer:"<<data_buffer.size()<<std::endl;
    std::cout<<"average:"<<average<<std::endl;
    std::cout<<"temp:"<<temp<<std::endl;
    filter_buffer_.clear();
    for(int i = start; i < data_buffer.size(); i++)
    {
        filter_buffer_.push_back((data_buffer.at(i).distance - average) / temp);
    }
}

int InitialStage::find_top_distance_peak(std::vector<double> &filter_buffer, double maximum_threshold, double duration_ms)
{
    double peak_value = 0.0;
    int peak_pos = 0;
    PeakZeroPair peak_zero_pos;
    double zero_pos = 0.0;
    double pre_diff = 0.0;
    double cur_diff = 0.0;
    double pre_distance = filter_buffer.at(0);
    std::vector<int> extreme_pos_list;              //极值
    double slot = duration_ms / (filter_buffer.size() - 1);
    int window_size = static_cast<int>(600 / slot);
    
    if(filter_buffer.empty())
        return -1;

    extreme_pos_list.clear();
    for (int i = 1; i < filter_buffer.size(); i++) 
    {
        cur_diff = filter_buffer.at(i) - pre_distance;
        if((pre_diff > 0.0) && (cur_diff <= 0.0) && 
            (filter_buffer.at(i) > maximum_threshold))       //找到一个极大值
        {
            extreme_pos_list.push_back(i);                 //记录极大值的位置
        }

        pre_distance = filter_buffer.at(i);
        pre_diff = cur_diff;
    }

    // for(int i = 0; i < extreme_pos_list.size(); i++)
    // {
    //     std::cout<<i<<" at "<< extreme_pos_list.at(i)<<std::endl;
    // }
    if(extreme_pos_list.size() <= 1)           //数据集内没有极大值，即找不到峰值
    {
        return -1;
    }

    // std::cout<<"data size:"<<data_buffer.size()<<std::endl;
    // std::cout<<"window_size:"<<window_size<<std::endl;
    peak_pos_list_.clear();
    peak_pos = extreme_pos_list.at(0);
    peak_value = filter_buffer.at(peak_pos);
    for(int i = 1; i < extreme_pos_list.size(); i++)
    {
        if(window_size < (extreme_pos_list.at(i) - peak_pos))   //0.6s内无极值，认为此处是一步峰值
        {
            peak_zero_pos.aft_zero = -1;
            peak_zero_pos.zero = -1;
            peak_zero_pos.peak = peak_pos;
            peak_pos_list_.push_back(peak_zero_pos);
            //重新设定峰值判定起点
            if((i + 1) < extreme_pos_list.size())
            {
                peak_pos = extreme_pos_list.at(i + 1);
                peak_value = filter_buffer.at(peak_pos);
            }
            else
            {
                return 0;
            }
        }
        else
        {
            if(peak_value < filter_buffer.at(extreme_pos_list.at(i)))
            {
                //如果后一个极值大于前一个，那么重新从后一个极值开始寻峰
                peak_pos = extreme_pos_list.at(i);
                peak_value = filter_buffer.at(peak_pos);
            }
        }
    }

    if((filter_buffer.size() - extreme_pos_list.back()) > window_size)
    {
        peak_zero_pos.zero = -1;
        peak_zero_pos.aft_zero = -1;
        peak_zero_pos.peak = peak_pos;
        peak_pos_list_.push_back(peak_zero_pos);
    }


    for(int i = 0; i < peak_pos_list_.size(); i++)
    {
        std::cout<<"peak "<<i<<" at "<< peak_pos_list_.at(i).peak<<":"
            <<filter_buffer.at(peak_pos_list_.at(i).peak)<<std::endl;
    }

    return 0;
}

int InitialStage::find_nearest_zero_point(std::vector<double> &filter_buffer)
{
    if(peak_pos_list_.empty())           //峰值前没有过零点
        return -1;

    double pre_distance;
    int pre_peak_edge = 0;              //峰值边界
    for(int i = 0; i < peak_pos_list_.size(); i++)
    {
        //先找峰值前的零点
        pre_distance = filter_buffer.at(peak_pos_list_.at(i).peak);
        for(int j = peak_pos_list_.at(i).peak; j > pre_peak_edge; j--)
        {
            if(((filter_buffer.at(j) <= 0) && (pre_distance > 0)) ||
               ((filter_buffer.at(j) < 0) && (pre_distance >= 0)))  //过零点
            {
                pre_peak_edge = peak_pos_list_.at(i).peak;
                peak_pos_list_.at(i).zero = j;
                break;
            }

            pre_distance = filter_buffer.at(j);
        }
    }

    //再找峰值后的零点
    pre_peak_edge = peak_pos_list_.at(1).peak;     //峰值边界
    for(int i = 0; i < peak_pos_list_.size(); i++)
    {
        pre_distance = filter_buffer.at(peak_pos_list_.at(i).peak);
        for(int j = peak_pos_list_.at(i).peak; j < pre_peak_edge; j++)
        {
            if(((filter_buffer.at(j) < 0) && (pre_distance >= 0)) ||
               ((filter_buffer.at(j) <= 0) && (pre_distance > 0)))   //过零点
            {
                if((i + 1) >= peak_pos_list_.size())
                {
                    pre_peak_edge = peak_pos_list_.size();
                }
                else
                {
                    pre_peak_edge = peak_pos_list_.at(i+1).peak;
                }
                peak_pos_list_.at(i).aft_zero = j;
                break;
            }

            pre_distance = filter_buffer.at(j);
        }
    }

    for(int i = 0; i < peak_pos_list_.size(); i++)
    {
        std::cout<<"zero-peak-aftzero "<<i<<" at: "
            <<peak_pos_list_.at(i).zero<<"-->"
            <<peak_pos_list_.at(i).peak<<"-->"
            <<peak_pos_list_.at(i).aft_zero<<std::endl;
    }

    return 0;
}

uint64_t InitialStage::find_T_hs_event(std::vector<DataInfo> &data_buffer, int start, int t_to, uint64_t region_ms, double slot_ms)
{
    double gyr_data = 0;
    int gyr_pos = 0;
    int64_t data_region = static_cast<int64_t>(region_ms / slot_ms) + t_to + start;

    gyr_pos = t_to + start;
    gyr_data = data_buffer.at(gyr_pos).gyr;
    for(int i = t_to + start; (i < data_region) && (i < data_buffer.size()); i++)
    {
        if(gyr_data < data_buffer.at(i).gyr)
        {
            gyr_pos = i;
            gyr_data = data_buffer.at(i).gyr;
        }
    }

    // std::cout<<"hs_pos:"<<gyr_pos<<std::endl;
    return static_cast<uint64_t>((gyr_pos - start) * slot_ms);
}
 BT::NodeStatus InitialStage::onRunning(){
    //开启端口数据流获取判断
    if (!getInput<sensorData>("sensor_input", tmp_sensor_data_filtered_)) {
        throw BT::RuntimeError("Missing required input 'InitialStage.sensor_input'");
    }
    // std::cout << "InitialStage: imu_euler=" << tmp_sensor_data_filtered_.imu_euler 
    //             << ", imu_gyr=" << tmp_sensor_data_filtered_.imu_gyr 
    //             << ", tof_distance=" << tmp_sensor_data_filtered_.tof_distance << std::endl;
    cnt_++;
    // 获取当前时间（毫秒精度）存储到黑板
    if (time_flag_){
        // auto now = std::chrono::steady_clock::now();
        // auto duration = now.time_since_epoch();
        // start_millis_ = static_cast<double>(std::chrono::duration_cast<std::chrono::milliseconds>(duration).count());
        current_millis_ = config().blackboard->get<uint64_t>("global_counter");
        current_millis_ *= 2;
        start_millis_ = current_millis_;
        // 存储到黑板
        setOutput("time_stamp", static_cast<uint64_t>(start_millis_));
        time_flag_ = false;
    }

    //控制初始时间段
    // auto now = std::chrono::steady_clock::now();
    // auto duration = now.time_since_epoch();
    // current_millis_ = std::chrono::duration_cast<std::chrono::milliseconds>(duration).count();
    current_millis_ = config().blackboard->get<uint64_t>("global_counter");
    current_millis_ *= 2;

    double tmp; // 秒
    // getInput<double>("initial_stage_duration",tmp);
    if (!getInput<double>("duration", tmp)) {
        throw BT::RuntimeError("Missing required input 'InitialStage.duration'");
    }
    // 将sensorData转换为vector<double>并添加到buffer_

    // 添加传感器数据（3个值）
    DataInfo row_data_;
    row_data_.tick = current_millis_;
    row_data_.euler = static_cast<double>(tmp_sensor_data_filtered_.imu_euler);
    row_data_.gyr = static_cast<double>(tmp_sensor_data_filtered_.imu_gyr);
    row_data_.distance = static_cast<double>(tmp_sensor_data_filtered_.tof_distance);
    buffer_.push_back(row_data_);  // 追加到尾部
    
    int ret;
    double data_slot;
    // mean_stance_duration_.push_back(0.0);
    start_millis_ = buffer_.at(start_pos_).tick;
    uint64_t elapsed_millis = current_millis_ - start_millis_;
    // std::cout<<"elapsed_millis:"<<elapsed_millis<<std::endl;
    if((tmp*1000.0) < (current_millis_ - buffer_.at(0).tick))
    {
        std::cout<<"find peak timeout"<<std::endl;
        print_all_outports(true);
        return BT::NodeStatus::SUCCESS;
    }
    if (elapsed_millis > (4000.0))//大于4s秒，基本有两步的数据了，开始判断
    {
        filter_laser_ranging(buffer_, start_pos_);
        //求取PKs
        ret = find_top_distance_peak(filter_buffer_, maximum_threshold_, elapsed_millis);
        if (ret != 0)  //找不到峰值
        {
            /* code */
            //重新计时，重新存3秒数据
            start_millis_ = current_millis_;
            print_all_outports(true);
            return BT::NodeStatus::RUNNING;
        }else{
            if(peak_pos_list_.size() < 2)       //只有一步，不够判定周期，继续等待下一步
            {
                start_pos_ += 1;
                return BT::NodeStatus::RUNNING;
            }
            ret = find_nearest_zero_point(filter_buffer_);
            if(ret != 0)        //没能找到过零点
            {
                start_millis_ = current_millis_;
                return BT::NodeStatus::RUNNING;
            }
            //计算零点位置的时刻点
            // data_slot = elapsed_millis * 1.0 / (buffer_.size() - 1);
            data_slot = 2.0;

            for(int i = 0; i < peak_pos_list_.size(); i++)
            {
                T_to_.push_back(static_cast<uint64_t>(peak_pos_list_.at(i).zero * data_slot) + start_millis_);
                T_hs_.push_back(find_T_hs_event(buffer_, start_pos_, peak_pos_list_.at(i).zero, 600, data_slot) + start_millis_);
            }
            
            mean_swing_duration_.push_back(T_hs_.at(0) - T_to_.at(0));
            mean_stance_duration_.push_back(T_to_.at(1) - T_hs_.at(0));
            mean_T_hs_potential_region_.push_back(T_hs_.at(0) - peak_pos_list_.at(0).aft_zero * data_slot - start_millis_);
            std::cout<<"start_millis_:"<<start_millis_<<std::endl;
            std::cout<<"T_to:"<<T_to_.back()<<std::endl;
            std::cout<<"swing:"<<T_hs_.at(0) - T_to_.at(0)<<std::endl;
            std::cout<<"stance:"<<T_to_.at(1) - T_hs_.at(0)<<std::endl;
            std::cout<<"region:"<<T_hs_.at(0) - peak_pos_list_.at(0).aft_zero * data_slot - start_millis_<<std::endl;
            //设置变量数值
            // T_hs_.at(0) = current_millis_;
            T_to_last_ = T_to_.back();
            T_hs_last_ = T_hs_.back();
            setOutput("T_to", T_to_last_);
            setOutput("T_hs", T_hs_last_);
            setOutput("init_mean_T_hs_potential_region", std::make_shared<std::vector<uint64_t>>(mean_T_hs_potential_region_));
            setOutput("init_mean_swing_duration",  std::make_shared<std::vector<uint64_t>>(mean_swing_duration_) );//数组转换成指针再过端口发出
            setOutput("init_mean_stance_duration", std::make_shared<std::vector<uint64_t>>(mean_stance_duration_) );//数组转换成指针再过端口发出
            //update_state();
            std::cout << "InitialStage_SUCCESS_"<<"elapsed_millis:"<<elapsed_millis<<" tmp: " <<tmp << std::endl;
            print_all_outports(true);
            return BT::NodeStatus::SUCCESS;
        }
        std::cout << "InitialStage_SUCCESS_"<<"elapsed_millis:"<<elapsed_millis<<" tmp: " <<tmp << std::endl;
        print_all_outports(true);
        return BT::NodeStatus::SUCCESS;
    }

    // std::cout << "InitialStage_onRunning()" << std::endl;
    return BT::NodeStatus::RUNNING;

 }

void InitialStage::print_vector(const std::vector<uint64_t>& vec, const std::string& name) {
    if (!vec.empty()) {
        std::cout << name << ": [";
        for (size_t i = 0; i < vec.size(); ++i) {
            if (i > 0) std::cout << ", ";
            std::cout << vec[i];
        }
        std::cout << "]" << std::endl;
    } else {
        std::cout << name << ": 向量为空" << std::endl;
    }
}
void InitialStage::print_var(const uint64_t& var, const std::string& name) {
    std::cout << name << ": " << var << std::endl;
}


void InitialStage::print_all_outports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== InitialStage节点端口输出值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_to_last_, "T_to_last_");
    print_var(T_hs_last_, "T_hs_last_");
    print_var(start_millis_, "time_stamp");
    print_vector(mean_T_hs_potential_region_, "mean_T_hs_potential_region_");
    print_vector(mean_swing_duration_, "mean_swing_duration_");
    print_vector(mean_stance_duration_, "mean_stance_duration_");
    std::cout << "======================" << std::endl;
}
void InitialStage::print_all_inports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== InitialStage节点端口输入值 ===" << std::endl;
    // 打印所有变量和向量
    // print_var(T_hs_, "T_hs_");
    // // 打印所有向量
    // print_vector(mean_stance_duration_, "mean_stance_duration_");
    std::cout << "======================" << std::endl;
}

















#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  std::cout << "Registering nodes...: InitialStage" << std::endl;//在宏内添加调试信息或条件判断
  factory.registerNodeType<InitialStage>("InitialStage");
}